/* Includes ------------------------------------------------------------------*/
#include "bluenrg_lpx.h"
#include "system_BlueNRG_LP.h"
#include "bluenrg_lp_api.h"
#include "OTA_btl.h"
#include "rf_driver_ll_flash.h"
#include "rf_driver_ll_rcc.h"
#include "rf_driver_ll_bus.h"


/* Defines -------------------------------------------------------------------*/
#define SECURE_BOOT_MAGIC_WORD                 0xEC1CC10B
#define SECURE_BOOT_OTP_ACTIVATION_ADDR        0x10001800
#define SECURE_BOOT_OTP_R2_ADDR                0x10001808
#define SECURE_BOOT_OTP_MODULUS_2048_ADDR      0x10001908
#define SECURE_BOOT_OTP_E_65537_ADDR           0x10001A08

#define CRL_SHA256_SIZE         32
#define E_SHA256                3
#define HASH_SUCCESS            0
#define SIGNATURE_VALID         1003
#define SIGNATURE_INVALID       1004
#define FAULT_RESISTANT_PATTERN 0x3A3A3A

#define RESET_WAKE_DEEPSLEEP_REASONS 0x05
#define TAG_VALUE(x)     (* ((volatile uint32_t*) ((x) + OTA_TAG_VECTOR_TABLE_ENTRY_OFFSET)))

/* Typedef -------------------------------------------------------------------*/
typedef struct secureBootS {
	uint32_t RESERVED[10];
	int32_t (*generate_SHA256_digest)(const uint8_t*, int32_t, uint8_t*,int32_t*);
	int32_t (*verify_RSA_precomputedR2)(const uint8_t*, uint32_t, const uint8_t*, uint32_t, const uint8_t*, const uint8_t*, const uint8_t*,uint8_t,int32_t*);
} secureBoot_type;

typedef  void (*pFunction)(void);
static uint8_t verifyFW(uint32_t appAddress) ;

static const uint8_t Modulus_2048[] = {174,57,43,185,25,171,217,85,130,10,123,188,66,32,252,202,43,145,172,200,180,170,118,192,105,121,84,252,195,122,9,20,69,135,177,194,153,44,152,138,111,174,96,78,68,14,171,127,174,141,80,70,128,189,24,107,91,40,59,19,249,48,32,235,241,52,33,75,8,83,59,220,67,50,96,129,140,82,155,114,211,16,56,162,41,25,116,21,56,191,23,105,100,100,100,10,243,110,78,124,78,136,235,90,18,244,235,77,107,248,56,124,120,57,100,35,125,150,179,103,31,190,70,41,17,152,155,231,83,126,57,47,201,71,90,18,191,62,242,230,216,248,212,144,48,137,111,106,245,134,188,248,12,235,104,254,235,67,182,163,122,221,166,88,89,98,91,129,177,9,158,213,177,102,249,255,85,161,166,55,189,187,253,247,140,38,103,61,241,199,36,16,208,212,189,227,210,166,106,195,252,121,82,62,208,208,32,87,4,157,234,254,214,44,189,51,85,193,7,45,60,112,96,197,230,104,77,57,32,198,113,172,94,196,51,251,18,89,112,76,242,22,179,150,46,188,96,149,45,167,243,238,47,90,191,193,};

static const uint8_t R2[] = {141,75,164,29,123,102,22,105,33,8,217,54,125,223,199,28,121,6,183,153,86,195,251,212,238,216,196,228,246,232,225,77,11,63,220,72,242,190,86,192,96,37,179,172,141,40,254,39,253,203,70,72,119,125,107,155,126,123,214,249,134,13,82,230,238,39,93,233,101,72,76,147,194,65,246,237,39,234,185,136,168,196,193,24,96,193,195,53,139,254,217,243,193,141,141,101,221,140,160,208,203,43,137,131,129,46,207,161,155,18,0,184,40,27,198,109,43,154,131,174,130,203,231,164,250,77,147,24,222,51,194,199,137,91,223,42,212,214,9,190,123,105,111,162,226,137,97,237,233,117,220,190,56,106,226,247,132,253,2,172,199,86,189,77,171,76,169,62,226,27,123,190,99,77,2,100,184,117,115,240,59,241,142,11,22,213,36,72,11,151,226,198,109,0,26,196,138,137,19,205,55,37,179,26,223,221,165,127,217,118,171,224,45,45,199,48,48,116,60,249,105,30,53,206,204,38,126,203,128,83,31,122,162,51,247,98,187,56,65,16,24,86,61,14,84,117,148,200,199,163,182,50,141,63,71,47,};

static const uint8_t e_65537[] = {0x00, 0x01, 0x00, 0x01};



static uint8_t OTA_Check_Storage_Area(uint32_t start_address, uint32_t end_address)
{
	volatile uint32_t *address; 
	uint32_t i; 
	
	for(i=start_address;i<end_address; i = i +4)
	{ 
		address = (volatile uint32_t *) i;
		if (*address != 0xFFFFFFFF)
			return 1; /* do flash erase */
	}
	
	return 0; /* no flash erase is required */
}
static void OTA_Erase_Storage_Area(uint16_t startPageNumber, uint16_t endPageNumber)
{
	LL_FLASH_Erase(FLASH, LL_FLASH_TYPE_ERASE_PAGES, (startPageNumber), (endPageNumber-startPageNumber+1));
}

static uint8_t DrvFlash_Write
(
uint32_t u32_Addr,
uint8_t * u8p_Data,
uint32_t u32_Len
)
{
	uint32_t u32_Data =0;
	uint32_t u32_temp_Round = 0 ;
	uint32_t u32_temp_remainder = 0 ;
	u32_temp_Round = u32_Len/4 ;
	u32_temp_remainder = u32_Len%4 ;


	for(uint32_t i =0 ; i < u32_temp_Round ;i+=1)
	{
		u32_Data = (((uint32_t)u8p_Data[i*4+3]<<24)|((uint32_t)u8p_Data[i*4+2]<<16)|((uint32_t)u8p_Data[i*4+1]<<8)|((uint32_t)u8p_Data[i*4])) ;
		LL_FLASH_Program(FLASH, u32_Addr, u32_Data) ;
		u32_Addr = u32_Addr + 4 ;
	}
	

	u32_Data = 0 ;
	for(uint32_t i=0 ; i<u32_temp_remainder ;i++)
	{
		u32_Data |= (uint32_t)u8p_Data[u32_temp_Round*4+i] << i*8 ;
	}

	LL_FLASH_Program(FLASH, u32_Addr, u32_Data) ;
	return 1;
}

static uint32_t OTA_Check_Application_Tags_Value(void)
{
	uint32_t appAddress = 0;
	if ( ((TAG_VALUE(APP_LOWER_ADDRESS) == OTA_INVALID_OLD_TAG) && (TAG_VALUE(APP_HIGHER_ADDRESS) == OTA_INVALID_OLD_TAG))   || /* 1 */
	    ((TAG_VALUE(APP_LOWER_ADDRESS) == OTA_VALID_TAG) && (TAG_VALUE(APP_HIGHER_ADDRESS) == OTA_INVALID_OLD_TAG))         || /* 2 */
		    ((TAG_VALUE(APP_LOWER_ADDRESS) == OTA_VALID_TAG) && (TAG_VALUE(APP_HIGHER_ADDRESS) == OTA_VALID_TAG))               || /* 4 */
			    ((TAG_VALUE(APP_LOWER_ADDRESS) == OTA_INVALID_OLD_TAG) && (TAG_VALUE(APP_HIGHER_ADDRESS) == OTA_IN_PROGRESS_TAG))   || /* 8 */
				    ((TAG_VALUE(APP_LOWER_ADDRESS) == OTA_VALID_TAG) && (TAG_VALUE(APP_HIGHER_ADDRESS) == OTA_IN_PROGRESS_TAG)))           /* 9 */  
	{
		/* Jump to Lower Application */
		appAddress = APP_LOWER_ADDRESS;
		if(verifyFW(appAddress))
		{
			if (OTA_Check_Storage_Area(APP_HIGHER_ADDRESS,APP_HIGHER_ADDRESS_END))
			{
				/* Erase OLD Higher application storage area */
				OTA_Erase_Storage_Area(OTA_HIGHER_APPLICATION_PAGE_NUMBER_START, OTA_HIGHER_APPLICATION_PAGE_NUMBER_END); 
				
			}
		}
		else
		{
			appAddress = APP_HIGHER_ADDRESS;
		}
	}
	else if ( ((TAG_VALUE(APP_LOWER_ADDRESS) == OTA_INVALID_OLD_TAG) && (TAG_VALUE(APP_HIGHER_ADDRESS) == OTA_VALID_TAG))       || /* 3 */
		 ((TAG_VALUE(APP_LOWER_ADDRESS) == OTA_IN_PROGRESS_TAG) && (TAG_VALUE(APP_HIGHER_ADDRESS) == OTA_INVALID_OLD_TAG)) || /* 6 */
			 ((TAG_VALUE(APP_LOWER_ADDRESS) == OTA_IN_PROGRESS_TAG) && (TAG_VALUE(APP_HIGHER_ADDRESS) == OTA_VALID_TAG)))         /* 7 */     
	{
		/* Jump to Higher Application */
		appAddress = APP_HIGHER_ADDRESS;
		if(verifyFW(appAddress))
		{
			if (OTA_Check_Storage_Area(APP_LOWER_ADDRESS,APP_LOWER_ADDRESS_END))
			{ 
				/* Erase OLD Lower application storage area */
				OTA_Erase_Storage_Area(OTA_LOWER_APPLICATION_PAGE_NUMBER_START, OTA_LOWER_APPLICATION_PAGE_NUMBER_END); 
			}
		}
		else
		{
			appAddress = APP_LOWER_ADDRESS;
		}
	}
	else if ((TAG_VALUE(APP_LOWER_ADDRESS) == OTA_IN_PROGRESS_TAG) && (TAG_VALUE(APP_HIGHER_ADDRESS) == OTA_IN_PROGRESS_TAG))   /* 5 */
	{
		/* 5: Is it possible? No. What to do?*/
	}
	return appAddress;
}



static uint8_t verifyFW(uint32_t appAddress)
{
	uint32_t FW_size;
	uint32_t *FW_input;
	uint32_t *FW_sign_2048;
	uint32_t Modulus_2048_size, e_65537_size;
	uint8_t FW_input_digest[CRL_SHA256_SIZE]; /* Buffer to store the computed
	digest: MUST be allocated
	before calling the SHA256
	functions */
	int32_t outSize,     /* Used to store the digest's length after its computation */
	doubleCheck;        /* Used to prevent fault attacks on verification step */
	int32_t retval;
	
	secureBoot_type *secureBoot = (secureBoot_type*)SYSTEM_MEMORY_BASE;
	
	/* Disable Peripheral Radio */
	LL_APB2_DisableClock(LL_APB2_PERIPH_MRBLE);
	/* Configure 1 FLASH Wait State */
	LL_FLASH_SetWaitStates(FLASH, LL_FLASH_WAIT_STATES_1);
	/* Setup the HSI frequency to 64 MHz */
	LL_RCC_SetRC64MPLLPrescaler(LL_RCC_RC64MPLL_DIV_1);
	
	FW_size = *(uint32_t*)(appAddress + 0x18);
	FW_input = (uint32_t*)appAddress;
	FW_sign_2048 = (uint32_t*)((uint8_t*)FW_input+FW_size);
	Modulus_2048_size = 256;
	e_65537_size = 4;
	
	/* -----------------------------------------------------------------------*
	* Generate digest of the customer's public key (SHA-256) -----------------*
	* ------------------------------------------------------------------------*/
	retval = secureBoot->generate_SHA256_digest((uint8_t*)FW_input,
						    FW_size,
						    FW_input_digest,
						    &outSize);
	if (retval != HASH_SUCCESS) {
		return 0 ;
	}
	
	/* -----------------------------------------------------------------------*
	* RSA PKCS#1 v1.5 Signature verification ---------------------------------*
	* ------------------------------------------------------------------------*/
	retval = secureBoot->verify_RSA_precomputedR2((uint8_t*)Modulus_2048, Modulus_2048_size,
						      (uint8_t*)e_65537, e_65537_size,
						      (uint8_t*)R2,
						      (uint8_t*)FW_sign_2048,
						      FW_input_digest,
						      E_SHA256,
						      &doubleCheck);
	/* Check the return code: the following double check is provided to be
	*  resistant to fault attacks 
	*/
	if ((retval != SIGNATURE_VALID) || ((retval ^ doubleCheck) != FAULT_RESISTANT_PATTERN)) {
		return 0 ;
	}
	return 1 ;
}




int main(void) 
{
	
	uint32_t JumpAddress, appAddress;
	pFunction Jump_To_Application;
	//先对A和B区验签进行验签
	//如何A区验签成功，B区也验签成功，就把B区覆盖到A区
	//如何A区验签成功，B区也验签不成功，就把B区擦除不覆盖
	//如何A区验签不成功，B区也验签成功，就把B区覆盖到A区
	uint32_t *HIGHER_input = (uint32_t*)APP_HIGHER_ADDRESS;

	if(verifyFW(APP_LOWER_ADDRESS))
	{
		if(verifyFW(APP_HIGHER_ADDRESS))
		{
			
			//擦除A区
			OTA_Erase_Storage_Area(OTA_LOWER_APPLICATION_PAGE_NUMBER_START, OTA_LOWER_APPLICATION_PAGE_NUMBER_END); 
			//覆盖
			DrvFlash_Write(APP_LOWER_ADDRESS,(uint8_t*)HIGHER_input ,*(uint32_t*)(APP_HIGHER_ADDRESS + 0x18)) ;
			//擦除B区
			OTA_Erase_Storage_Area(OTA_HIGHER_APPLICATION_PAGE_NUMBER_START, OTA_HIGHER_APPLICATION_PAGE_NUMBER_END);
		}
		else
		{
			//不覆盖
		}
	}
	else
	{
		if(verifyFW(APP_HIGHER_ADDRESS))
		{
			
			//擦除A区
			OTA_Erase_Storage_Area(OTA_LOWER_APPLICATION_PAGE_NUMBER_START, OTA_LOWER_APPLICATION_PAGE_NUMBER_END); 
			//覆盖
			DrvFlash_Write(APP_LOWER_ADDRESS,(uint8_t*)HIGHER_input ,*(uint32_t*)(APP_HIGHER_ADDRESS + 0x18)) ;
			//擦除B区
			OTA_Erase_Storage_Area(OTA_HIGHER_APPLICATION_PAGE_NUMBER_START, OTA_HIGHER_APPLICATION_PAGE_NUMBER_END);
			
		}
		else
		{
			//不覆盖
		}
	}

	appAddress = APP_LOWER_ADDRESS ;
	if (appAddress == 0) {
		/* This case indicates that no valid application is present and this normally should not happen */
		while (1);
	}

	JumpAddress = *(__IO uint32_t*) (appAddress + 4);
	Jump_To_Application = (pFunction) JumpAddress;
	__set_MSP(*(__IO uint32_t*) appAddress);
	Jump_To_Application();
	while (1)
	{
	}
}

